///////////////////////////////////////////////////////////////////////////////
//
// Lynx 6 Arm Service - Service Types
//
// Based on code from the Lynxmotion web site arm.1.2.zip
// Modified with the permission of Lynxmotion
//
// Updated Nov-2007 by Trevor Taylor, Software Technology
//
// Change Log:
// Changed the contract identifier to reflect new version and
// also matched the manifest with the id (was wrong).
//
// Renumbered the joints and added the gripper. The joint numbers are
// now in an enum and it *should* be possible to change them, but this
// has not been tested. (See bottom of this file.)
//
// Added MaxAngularSpeed to limit arm movement speed.
//
// Added Angles as an internal cache and also to make them clearly
// visible in the Lynx6Arm state if you use a web browser. Who can
// read a Quaternion anyway?
//
// Rebuilt Feb 2008 with a new Contract Id and Namespace for ProMRDS release
//
///////////////////////////////////////////////////////////////////////////////

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Robotics.PhysicalModel.Proxy;

using System;
using System.Collections.Generic;
using System.ComponentModel;
using W3C.Soap;

using lynx6arm = ProMRDS.Robotics.Lynxmotion.Lynx6Arm;
using armproxy = Microsoft.Robotics.Services.ArticulatedArm.Proxy;

namespace ProMRDS.Robotics.Lynxmotion.Lynx6Arm
{  
    public static class Contract
    {
        // TT Nov-2007 - Changed to match the manifest, and then changed again
        // to a new month so that it does not conflict with the old service
        public const string Identifier = "http://www.promrds.com/contracts/2008/01/lynx6arm.html";
    }

    public class Lynx6ArmOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get>
    {
    }

    [DisplayName("Get")]
    [Description("Gets the arm's current state.")]
    public class Get : Get<GetRequestType, PortSet<Lynx6ArmState, Fault>> { }

    [DataContract]
    public class Lynx6ArmState : armproxy.ArticulatedArmState
    {
        // TT - Add another joint for the Gripper
        internal const int NUM_JOINTS = 6;

        //Each angle ranges from -90 to 90
        //Each joint speed is in uS per second 
        private int _gripperAngle;
        private int _gripperSpeed;

        [DataMember]
        public int GripperAngle
        {
            get { return _gripperAngle; }
            set { _gripperAngle = value; }
        }

        [DataMember]
        public int GripperSpeed
        {
            get { return _gripperSpeed; }
            set { _gripperSpeed = value; }
        }

        // TT Nov-2007 -
        // The angles of all the joints are kept here for convenience
        // because getting them from the arm is time consuming and a
        // pain in the neck because it uses quaternions!
        private float[] _angles;

        [DataMember]
        public float[] Angles
        {
            get { return _angles; }
            set { _angles = value; }
        }

        // TT Nov-2007 -
        // The maximum angular speed that is set in degrees/second
        // This sets a limit on how fast the arm can move
        // The arm service calculates the necessary time period for
        // each move in order to avoid sudden jumps
        private float _maxAngularSpeed;

        [DataMember]
        public float MaxAngularSpeed
        {
            get { return _maxAngularSpeed; }
            set { _maxAngularSpeed = value; }
        }

        public Lynx6ArmState()
            : base()
        {
            _gripperAngle = 0;
            _gripperSpeed = 500;
            // TT - Change this to make the arm move faster/slower
            // NOTE: At very slow speeds, the arm might be jerky
            _maxAngularSpeed = 90.0f;
            _angles = new float[NUM_JOINTS];
        }
    }

    // TT Nov-2007 -
    // These are the joint numbers that correspond to the
    // servos on the SSC-32 board. It *should* be possible
    // to change them without affecting the code, but this
    // has not been tested.
    [DataContract]
    public enum JointNumbers
    {
        Base = 0,
        Shoulder,
        Elbow,
        Wrist,
        WristRotate,
        Gripper
    };

}
